function mpc_controller = setup_mpc(params, dt)
%SETUP_MPC 设置MPC控制器
%
% 输入:
%   params - 参数结构体
%   dt - 采样时间
%
% 输出:
%   mpc_controller - MPC控制器对象

    % 线性化的状态空间模型 (在某个工作点附近)
    % 这里使用简化的线性化模型
    % 状态: [x, y, theta, v]
    % 控制: [a, delta]
    
    % 在标称速度v0处线性化
    v0 = params.v_nominal;
    
    A = [0, 0, -v0, 1, 0;
         0, 0, v0, 0, 0;
         0, 0, 0, 0, v0/params.L;
         0, 0, 0, 0, 1;
         0, 0, 0, 0, 0];  % 增广状态用于积分作用
    
    B = [0, 0;
         0, 0;
         0, 0;
         1, 0;
         0, 0];
    
    C = [1, 0, 0, 0, 0;
         0, 1, 0, 0, 0;
         0, 0, 1, 0, 0;
         0, 0, 0, 1, 0];
    
    D = zeros(4, 2);
    
    % 创建状态空间模型
    sys = ss(A, B, C, D);
    sys_d = c2d(sys, dt);  % 离散化
    
    % 创建MPC对象
    mpc_controller = mpc(sys_d, dt);
    mpc_controller.PredictionHorizon = params.mpc_horizon;
    mpc_controller.ControlHorizon = params.mpc_control_horizon;
    
    % 设置权重
    mpc_controller.Weights.OutputVariables = diag(params.Q);
    mpc_controller.Weights.ManipulatedVariablesRate = diag(params.R);
    
    % 设置约束
    % 控制输入约束
    mpc_controller.ManipulatedVariables(1).Min = params.a_min;
    mpc_controller.ManipulatedVariables(1).Max = params.a_max;
    mpc_controller.ManipulatedVariables(2).Min = params.delta_min;
    mpc_controller.ManipulatedVariables(2).Max = params.delta_max;
    
    % 输出约束
    mpc_controller.OutputVariables(4).Min = params.v_min;
    mpc_controller.OutputVariables(4).Max = params.v_max;
end